package ru.ifmo;

import lejos.hardware.Sound;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.Font;
import lejos.hardware.lcd.GraphicsLCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.Button;
import lejos.robotics.navigation.DifferentialPilot;


/**
 * Created by Admin on 15.12.14.
 */
public class LegoRobot {
    private EV3 brick;
    private GraphicsLCD lcd;
    private EV3MediumRegulatedMotor grabberMotor;
    private EV3LargeRegulatedMotor leftMotor;
    private EV3LargeRegulatedMotor rightMotor;
    private SmartSpaceKP kp;
    private boolean endScenario;
    public static volatile boolean stopGrabber;

    private MotorCaptureThread motorCaptureThread;
    GrabberListener handler;
    DifferentialPilot pilot;


    public LegoRobot(EV3 brick) {
        this.brick = brick;

        lcd = this.brick.getGraphicsLCD();
        lcd.clear();
        lcd.setFont(Font.getDefaultFont());
        lcd.drawString("Success!", 9, 1, 0);

        leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
        rightMotor = new EV3LargeRegulatedMotor(MotorPort.C);
        grabberMotor = new EV3MediumRegulatedMotor(MotorPort.D);

        stopGrabber = false;

        pilot = new DifferentialPilot(5.3f, 10.3f, leftMotor, rightMotor);

        pilot.setRotateSpeed(45);
        pilot.setTravelSpeed(7);

        try {
            kp = new SmartSpaceKP("192.168.1.226", 10010, "X", false);
            handler = new GrabberListener(this);
            kp.subscribeData(new SmartSpaceTriplet("SortingLine", "hasStatus", "activated"), handler);
        } catch (SmartSpaceException e) {
            e.printStackTrace();
        }


        moveCaptureOff(); // клешни поднимаем
    }

    public void exitScenario() {
        try {
            kp.removeData(new SmartSpaceTriplet("Grabber", "hasStatus", "activated"));
        } catch (SmartSpaceException e) {
            e.printStackTrace();
        }

        try {
            kp.leaveSmartSpace();
        } catch (SmartSpaceException e) {
            e.printStackTrace();
        }
    }

    public void subscribeData() {
        synchronized (kp) {
            try {
                kp.subscribeData(new SmartSpaceTriplet("Object", "hasColor", null), handler);
            } catch (SmartSpaceException e) {
                e.printStackTrace();
            }
            try {
                kp.subscribeData(new SmartSpaceTriplet("Object", "hasStatus", "arrived"), handler);
            } catch (SmartSpaceException e) {
                e.printStackTrace();
            }

            try {
                kp.insertData(new SmartSpaceTriplet("Grabber", "hasStatus", "activated"));
            } catch (SmartSpaceException e) {
                e.printStackTrace();
            }
        }
    }

    public void setEndScenario(boolean endScenario) {
        this.endScenario = endScenario;
    }

    public void goodbye() {
        lcd.drawString("Goodbye!", 9, 1, 0);
    }

    private void moveForward(int speed, int degrees) {
        leftMotor.setSpeed(speed);
        rightMotor.setSpeed(speed);

        leftMotor.rotate(degrees, true);
        rightMotor.rotate(degrees, true);

        while (leftMotor.isMoving() && rightMotor.isMoving()) {
        }

        leftMotor.stop();
        rightMotor.stop();
    }

    private void moveBackward(int speed, int degrees) {
        moveForward(speed, -degrees);
    }

    public void moveRobotToSortingLine() {
        pilot.travel(10);
         //moveForward(90, (int)(2.5*90));
    }

    public void moveRobotBack() {
        pilot.travel(-10);
        //moveBackward(90, (int)(2.5*90));
    }

    private void turnAround(int speed, int degrees) {
        leftMotor.setSpeed(speed);
        rightMotor.setSpeed(speed);

        leftMotor.rotate(degrees, true);
        rightMotor.rotate(-degrees, true);

        while (leftMotor.isMoving() && rightMotor.isMoving()) {

        }

        leftMotor.stop();
        rightMotor.stop();
    }

    public void turnToRight() {
        pilot.rotate(-90);
        //turnAround(90, 172);
    }

    public void turnToLeft() {
        pilot.rotate(90);
        //turnAround(90, -172);
    }

    public void moveCaptureOff() {
        grabberMotor.setSpeed(40);
        grabberMotor.rotate(-40);

        grabberMotor.stop();
        //lcd.drawString("End!", 9, 30, 0);
    }

    public void startScenario(int pathLength) throws InterruptedException {

        moveRobotToSortingLine(); // подъезд к линии

        motorCaptureThread = new MotorCaptureThread(grabberMotor);
        motorCaptureThread.start();

        Thread.sleep(1500);

        moveRobotBack();
        turnToRight();
        pilot.travel(pathLength);
        //moveForward(200, pathLength);
        turnToLeft();
        Thread.sleep(300);
        stopGrabber = true;
        motorCaptureThread.join();
        turnToLeft();
        pilot.travel(pathLength);
        //moveForward(200, pathLength);
        turnToRight();

        endScenario = true;
    }
}

class MotorCaptureThread extends Thread {

    private EV3MediumRegulatedMotor motor;

    public MotorCaptureThread(EV3MediumRegulatedMotor motor) {
        this.motor = motor;
    }

    @Override
    public void run() {
        motor.setSpeed(50);
        motor.setAcceleration(3000);
        motor.forward();

        while (!LegoRobot.stopGrabber) {

        }

        Sound.beep();

        motor.stop();

        motor.setSpeed(40);
        motor.rotate(-40);

        motor.stop();

        Sound.beep();
    }
}
